﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Net.Sockets;
using System.Net;

namespace WinRos
{
    class WinRos
    {
        private Socket connection;
        private IPAddress host;
        private IPEndPoint hostep;

        public WinRos(string theAddress)
        {
            if (theAddress == "" )
            {
                //defualt netbook
                host = IPAddress.Parse("192.168.1.66");
                hostep = new IPEndPoint(host, 9090);
                connection = new Socket(AddressFamily.InterNetwork,
                                        SocketType.Stream, ProtocolType.Tcp);
                connection.SetSocketOption(SocketOptionLevel.Socket, SocketOptionName.ReuseAddress, true);
            }
            else
            {
                host = IPAddress.Parse(theAddress);
                hostep = new IPEndPoint(host, 9090);
                connection = new Socket(AddressFamily.InterNetwork,
                                                   SocketType.Stream, ProtocolType.Tcp);
                connection.SetSocketOption(SocketOptionLevel.Socket, SocketOptionName.ReuseAddress, true);
            }

            try
            {
                Console.WriteLine("Attemping to connect....");
                //closes connection if opened somehow
                if (connection.Connected)
                {
                    connection.Close();
                }
                connection.Connect(hostep);
                Console.WriteLine("Connected!!");
            }
            catch (SocketException e)
            {
                Console.WriteLine("Problem connecting to host");
                Console.WriteLine(e.ToString());
                connection.Close();
                return;
            }

            //performs the handshake
            this.handShake();
        }

        ///<name>
        ///handShake
        ///</name>
        ///<summary>
        ///Perfoms the handshake for the socket connection
        ///</summary>
        ///<parameters>
        ///None
        ///</parameters>
        ///<return>
        ///None
        ///</return>
        private void handShake()
        {
            String handshake = "raw\r\n\r\n";

            int i = this.connection.Send(Encoding.UTF8.GetBytes(handshake));
            Console.WriteLine(" The handshake has been sent and it is {0} bytes ", i);
        }
        
        ///<name>
        ///publish
        ///</name>
        ///<summary>
        ///Sends command to rosbridge 
        ///</summary>
        ///<parameters>
        ///JSON formatted string
        ///</parameters>
        ///<return>
        ///None
        ///</return>
        ///<remarks>
        ///A known issue is that Rosbridge sometimes drops messages that it receives
        ///If there are issues you can multiply the messages over the socket
        ///</remarks>
        public void publish(string message)
        {

            int i = 0;
                
            i += this.connection.Send(new byte[] { 0 });
            i += this.connection.Send(Encoding.UTF8.GetBytes(message));
            i += this.connection.Send(new byte[] { 255 });

            //if there are issues with the application just send the message multiple times
            //i.e 
            // for(int j = 0; j < 2; j++)
            // {
            //   i += this.connection.Send(new byte[] { 0 });
            //   i += this.connection.Send(Encoding.UTF8.GetBytes(message));
            //   i += this.connection.Send(new byte[] { 255 });
            // }

            Console.WriteLine(" The message has been sent and it is {0} bytes ", i);
        }

        ///<name>
        ///disconnect
        ///</name>
        ///<summary>
        ///Disconnects the socket connection
        ///</summary>
        ///<parameters>
        ///None
        ///</parameters>
        ///<return>
        ///None
        ///</return>
        public void disconnect()
        {
            this.connection.Disconnect(true);
        }
    }
}
